#pragma ident "$Id XY. CAO. 2016-11-24 $"

//============================================================================
//
//  Copyright (c)
//
//  XY. CAO, Wuhan Uniersity, 2016
//
//  hold the raw type id from rinex: C1C, C1W, C2W, C5Q, C5X, ...
//  SJ. Zhang, Wuhan University, 2019.6.1
//
//============================================================================

#include "MGEXDCBCorrection.hpp"

using namespace std;

namespace gpstk
{

      // Returns a string identifying this object.
   string MGEXDCBCorrection::getClassName() const
   { return "MGEXDCBCorrection"; }

   
      /* Returns a satTypeValueMap object, adding the new data generated
       *  when calling this object.
       *
       * @param time      Epoch corresponding to the data.
       * @param gData     Data object holding the data.
       */
   satTypeValueMap& MGEXDCBCorrection::Process( const CommonTime& time,
                                                satTypeValueMap& gData )
      noexcept(false)
   {
	   try
	   {

		   SatIDSet satRejectedSet;

		   // Loop through all the satellites
		   satTypeValueMap::iterator it;
		   for (it = gData.begin(); it != gData.end(); ++it)
		   {
			   SatID sat = it->first;


			   if(sat.system == SatelliteSystem::GPS)
			   {
				   double GPS_a = L1_FREQ_GPS*L1_FREQ_GPS/
					   ( L1_FREQ_GPS*L1_FREQ_GPS - L2_FREQ_GPS*L2_FREQ_GPS);
				   double GPS_b = L2_FREQ_GPS*L2_FREQ_GPS/
					   ( L1_FREQ_GPS*L1_FREQ_GPS - L2_FREQ_GPS*L2_FREQ_GPS);

                   bool findC1C(false);
                   bool findC1W(false);
                   bool findC2W(false);
                   bool findC2P(false);
                   bool findC5Q(false);
                   bool findC5X(false);

                   typeValueMap::iterator itC1W( it->second.find(TypeID::C1W) );
                   typeValueMap::iterator itC1C( it->second.find(TypeID::C1C) );
                   typeValueMap::iterator itC2W( it->second.find(TypeID::C2W) );
                   typeValueMap::iterator itC2P( it->second.find(TypeID::C2P) );
                   typeValueMap::iterator itC5Q( it->second.find(TypeID::C5Q) );
                   typeValueMap::iterator itC5X( it->second.find(TypeID::C5X) );


                   findC1W = (itC1W != (*it).second.end());
                   findC1C = (itC1C != (*it).second.end());
                   findC2W = (itC2W != (*it).second.end());
                   findC2P = (itC2P != (*it).second.end());
                   findC5Q = (itC5Q != (*it).second.end());
                   findC5X = (itC5X != (*it).second.end());

				   // Second, find the DCB(Not all)
				   double GPS_c1cc1w(0.0), GPS_c1wc2w(0.0);
				   double GPS_c1cc5q(0.0), GPS_c1cc5x(0.0);
				   try
				   {
                       if(mgexDCBStore!=NULL)
                       {
					       // For dual-frequency
                           try
                           {
					           GPS_c1cc1w = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C1W").value;
                           }
					       catch(DCBTypeNotFound &e)
					       {
                               cerr << "MGEXDCBCorrection::dcb not found for current sat" << sat<< endl;
                               exit(-1);
					       }

					       // Since 2017, DLR do not provide the C1W-C2W of GPS
					       try
					       {
					           GPS_c1wc2w = (*mgexDCBStore).getSatDCB(time,sat,"C1W-C2W").value;
					       }
					       catch(DCBTypeNotFound &e)
					       {
					           double GPS_c1cc2w = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C2W").value;
					           GPS_c1wc2w = GPS_c1cc2w - GPS_c1cc1w;
					       }


                           if(findC1W)
                           {

				               it->second[TypeID::C1] = it->second[TypeID::C1W];
/*
				               // C1W->C1W/C2W IF
				               it->second[TypeID::C1] += GPS_b*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }
                           else if(findC1C)
                           {
                               // C1C->C1W
				               it->second[TypeID::C1]  = (*itC1C).second - GPS_c1cc1w*C_MPS*1.0E-9;
/*
				               // C1W->C1W/C2W IF
                               it->second[TypeID::C1] += GPS_b*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }

				           // Do nothing because all RINEX3 observe file contain
				           // GPS C2W and 'W' has the highest priority.			   
                           if(findC2W)
                           {

				               it->second[TypeID::C2] = it->second[TypeID::C2W];
/*
				               // C2W->C1W/C2W IF
				               it->second[TypeID::C2] += GPS_a*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }
                           else if(findC2P)
                           {

				               it->second[TypeID::C2] = it->second[TypeID::C2P];
/*
				               // C2P->C1W/C2W IF
				               it->second[TypeID::C2] += GPS_a*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }

				           try
				           {
				               // For dual-frequency
				               GPS_c1cc5x = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C5X").value;

				           }
				           catch(...)
				           {
				               continue;
				           }

				           try
				           {
				               // For dual-frequency
				               GPS_c1cc5q = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C5Q").value;

				           }
				           catch(...)
				           {
				               continue;
				           }

				           // If have L5 frequency observable
                           if(findC5Q)
                           {

				               it->second[TypeID::C5] = it->second[TypeID::C5Q];

				               // C1W-C5Q = -[C1C-C1W] + [C1C-C5Q]
				               it->second[TypeID::C5] += (GPS_c1cc5q - GPS_c1cc1w)*C_MPS*1.0E-9;

/*
				               // C5Q->C1W/C2W IF
				               it->second[TypeID::C5] += GPS_b*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }
                           else if(findC5X)
                           {

				               // C1W-C5X = -[C1C-C1W] + [C1C-C5X]
				               it->second[TypeID::C5] = it->second[TypeID::C5X];
				               it->second[TypeID::C5] += (GPS_c1cc5x - GPS_c1cc1w)*C_MPS*1.0E-9;
/*
				               // C5X->C1W/C2W IF
				               it->second[TypeID::C5] += GPS_b*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }

                       }
                       // if you want to processing only dual-frequency noncc receiver, 
                       // no dcb file is needed.
                       else
                       {
                           if(findC1W)
                           {

				               it->second[TypeID::C1] = it->second[TypeID::C1W];
/*
				               // C1W->C1W/C2W IF
				               it->second[TypeID::C1] += GPS_b*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }

				           // Do nothing because all RINEX3 observe file contain
				           // GPS C2W and 'W' has the highest priority.			   
                           if(findC2W)
                           {

				               it->second[TypeID::C2] = it->second[TypeID::C2W];
/*
				               // C2W->C1W/C2W IF
				               it->second[TypeID::C2] += GPS_a*GPS_c1wc2w*C_MPS*1.0E-9;
*/
                           }

                       }
					   
				   }
				   catch(...)
				   {
                       cout << "reject:" << sat << endl;
					   satRejectedSet.insert(sat);
					   continue;
				   }
			   }
			   else if(sat.system == SatelliteSystem::GLONASS)
			   {
////			   double GLO_a = L1_FREQ_GLO*L1_FREQ_GLO/
////				   ( L1_FREQ_GLO*L1_FREQ_GLO - L2_FREQ_GLO*L2_FREQ_GLO);
////			   double GLO_b = L2_FREQ_GLO*L2_FREQ_GLO/

////				   ( L1_FREQ_GLO*L1_FREQ_GLO - L2_FREQ_GLO*L2_FREQ_GLO);
////			   // First, find the tracking code
////			   char tracking1 = it->second[TypeID::R1TC];
////			   char tracking2 = it->second[TypeID::R2TC];
////			   // Nothing to do with GLONASS R3
////			   // char tracking3 = it->second[TypeID::R3TC];

////			   // Second, find the DCB(Not all)
////			   double GLO_c1cc1p(0.0), GLO_c1pc2p(0.0),GLO_c2cc2p(0.0);
////			   try
////			   {
////				   GLO_c1cc1p = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C1P").value;
////				   // Since 2017, DLR do not provide the C1P-C2P and C2C-C2P of GLO
////				   try
////				   {
////					   GLO_c1pc2p = (*mgexDCBStore).getSatDCB(time,sat,"C1P-C2P").value;
////					   GLO_c2cc2p = (*mgexDCBStore).getSatDCB(time,sat,"C2C-C2P").value;
////				   }
////				   catch(DCBTypeNotFound &e)
////				   {
////					   double GLO_c1cc2c = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C2C").value;
////					   double GLO_c1cc2P = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C2P").value;
////					   GLO_c1pc2p = GLO_c1cc2P - GLO_c1cc1p;
////					   GLO_c2cc2p = GLO_c1cc2P - GLO_c1cc2c;
////				   }
////			   }
////			   catch(...)
////			   {
////				   satRejectedSet.insert(sat);
////				   continue;
////			   }


////			   // Then correct dcb for GLONASS L1
////			   if(tracking1 == 'C') // C(100%)
////			   {
////				   it->second[TypeID::P1] -= GLO_c1cc1p*C_MPS*1.0E-9; // C1C->C1P
////				   it->second[TypeID::P1] += GLO_b*GLO_c1pc2p*C_MPS*1.0E-9;
////				   // C1P->C1P/C2P IF
////			   }
////			   else  if(tracking1 == 'P')// P
////			   {
////				   it->second[TypeID::P1] += GLO_b*GLO_c1pc2p*C_MPS*1.0E-9;
////				   // C1P->C1P/C2P IF
////			   }
////			   else
////			   {
////				   satRejectedSet.insert(sat);
////				   continue;
////			   }

////			   // Then correct dcb for glonass L2
////			   if(tracking2 == 'C') // C
////			   {
////				   it->second[TypeID::P2] -= GLO_c2cc2p*C_MPS*1.0E-9; // C2C->C2P
////				   it->second[TypeID::P2] += GLO_a*GLO_c1pc2p*C_MPS*1.0E-9;
////				   // C2P->C1P/C2P IF
////			   }
////			   else  if(tracking2 == 'P')// P(100%)
////			   {
////				   it->second[TypeID::P2] += GLO_a*GLO_c1pc2p*C_MPS*1.0E-9;
////				   // C1P->C1P/C2P IF
////			   }
////			   else
////			   {
////				   satRejectedSet.insert(sat);
////				   continue;
////			   }

////			   // No DCB for GLONASS L1-L3

			   }
			   else if(sat.system == SatelliteSystem::Galileo)
			   {
                   // E1 :L1_FREQ_GAL
                   // E5a:L5_FREQ_GAL
                   // E5b:L7_FREQ_GAL
    			   double GAL_a = L1_FREQ_GAL*L1_FREQ_GAL/
    				   ( L1_FREQ_GAL*L1_FREQ_GAL - L5_FREQ_GAL*L5_FREQ_GAL);
    			   double GAL_b = L5_FREQ_GAL*L5_FREQ_GAL/
    				   ( L1_FREQ_GAL*L1_FREQ_GAL - L5_FREQ_GAL*L5_FREQ_GAL);

                   bool findC1C(false);
                   bool findC1X(false);
                   bool findC5Q(false);
                   bool findC5X(false);
                   bool findC7Q(false);
                   bool findC7X(false);
                   bool findC8Q(false);
                   bool findC8X(false);
                   bool findC6Q(false);
                   bool findC6X(false);

                   typeValueMap::iterator itC1C( it->second.find(TypeID::C1C) );
                   typeValueMap::iterator itC1X( it->second.find(TypeID::C1X) );
                   typeValueMap::iterator itC5Q( it->second.find(TypeID::C5Q) );
                   typeValueMap::iterator itC5X( it->second.find(TypeID::C5X) );
                   typeValueMap::iterator itC7Q( it->second.find(TypeID::C7Q) );
                   typeValueMap::iterator itC7X( it->second.find(TypeID::C7X) );
                   typeValueMap::iterator itC8Q( it->second.find(TypeID::C8Q) );
                   typeValueMap::iterator itC8X( it->second.find(TypeID::C8X) );
                   typeValueMap::iterator itC6Q( it->second.find(TypeID::C6Q) );
                   typeValueMap::iterator itC6X( it->second.find(TypeID::C6X) );


                   findC1C = (itC1C != (*it).second.end());
                   findC1X = (itC1X != (*it).second.end());
                                   
                   findC5Q = (itC5Q != (*it).second.end());
                   findC5X = (itC5X != (*it).second.end());
                                   
                   findC7Q = (itC7Q != (*it).second.end());
                   findC7X = (itC7X != (*it).second.end());
                                   
                   findC8Q = (itC8Q != (*it).second.end());
                   findC8X = (itC8X != (*it).second.end());
                                   
                   findC6Q = (itC6Q != (*it).second.end());
                   findC6X = (itC6X != (*it).second.end());

    			   // Second, find the DCB(Not all)
    			   double GAL_c1cc5q(0.0), GAL_c1cc7q(0.0),GAL_c1cc8q(0.0);
    			   double GAL_c1xc5x(0.0), GAL_c1xc7x(0.0),GAL_c1xc8x(0.0);

                   if(mgexDCBStore!=NULL)
                   {
    			       try
    			       {
    			           // for dual-frequency
    			           GAL_c1cc5q = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C5Q").value;
    			           GAL_c1xc5x = (*mgexDCBStore).getSatDCB(time,sat,"C1X-C5X").value;
    			           GAL_c1cc7q = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C7Q").value;
    			           GAL_c1cc8q = (*mgexDCBStore).getSatDCB(time,sat,"C1C-C8Q").value;
    			           GAL_c1xc7x = (*mgexDCBStore).getSatDCB(time,sat,"C1X-C7X").value;
    			           GAL_c1xc8x = (*mgexDCBStore).getSatDCB(time,sat,"C1X-C8X").value;

    			       }
    			       catch(...)
    			       {
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }

    			       // Then correct dcb for GALILEO L1
    			       if( findC1C) // C
    			       {

    			           it->second[TypeID::C1] = it->second[TypeID::C1C];
    			           // C1C->C1C/C5Q IF
    			           it->second[TypeID::C1] += GAL_b*GAL_c1cc5q*C_MPS*1.0E-9;
    			       }
    			       else if( findC1X ) // X
    			       {
    			           it->second[TypeID::C1] = it->second[TypeID::C1X];
    			           // C1X->C1X/C5X IF
    			           it->second[TypeID::C1] += GAL_b*GAL_c1xc5x*C_MPS*1.0E-9;
    			       }
    			       else
    			       {
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }

    			       // Then correct dcb for GALILEO L5
    			       if(findC5Q) // C
    			       {
    			           it->second[TypeID::C5] = it->second[TypeID::C5Q];
    			           // C5Q->C1C/C5Q IF
    			           it->second[TypeID::C5] += GAL_a*GAL_c1cc5q*C_MPS*1.0E-9;
    			       }
    			       else if(findC5X) // X
    			       {
    			           it->second[TypeID::C5] = it->second[TypeID::C5X];
    			           // C5X->C1X/C5X IF
    			           it->second[TypeID::C5] += GAL_a*GAL_c1xc5x*C_MPS*1.0E-9;
    			       }
    			       else
    			       {
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }

    			       // Then correct dcb for GALILEO L7
    			       if(findC7Q ) // C
    			       {
    			           it->second[TypeID::C7] = it->second[TypeID::C7Q];
    			           // C7Q->C1C/C5Q IF
    			           it->second[TypeID::C7] += GAL_c1cc7q*C_MPS*1.0E-9 + GAL_b*GAL_c1cc5q*C_MPS*1.0E-9;
    			       }
    			       else if(findC7X) // X
    			       {
    			           it->second[TypeID::C7] = it->second[TypeID::C7X];
    			           // C7X->C1X/C5X IF
    			           it->second[TypeID::C7] += GAL_c1xc7x*C_MPS*1.0E-9 + GAL_b*GAL_c1xc5x*C_MPS*1.0E-9;
    			       }

    			       // Then correct dcb for GALILEO L8
    			       if(findC8Q) // C
    			       {
    			           it->second[TypeID::C8] = it->second[TypeID::C8Q];
    			           // C8Q->C1C/C5Q IF
    			           it->second[TypeID::C8] += GAL_c1cc8q*C_MPS*1.0E-9 + GAL_b*GAL_c1cc5q*C_MPS*1.0E-9;
    			       }
    			       else if(findC8X ) // X
    			       {
    			           it->second[TypeID::C8] = it->second[TypeID::C8X];
    			           it->second[TypeID::C8] += GAL_c1xc8x*C_MPS*1.0E-9 + GAL_b*GAL_c1xc5x*C_MPS*1.0E-9;
    			           // C8X->C1X/C5X IF
    			       }

                   }
                   else
                   {
                       if(findC1C)
                       {

				           it->second[TypeID::C1] = it->second[TypeID::C1C];
                       }

				       // Do nothing because all RINEX3 observe file contain
				       // GPS C2W and 'W' has the highest priority.			   
                       if(findC5Q)
                       {

				           it->second[TypeID::C5] = it->second[TypeID::C5Q];
                       }
                   }


			   }
			   else if(sat.system == SatelliteSystem::BDS)
			   {
    			   double BDS_a = L2_FREQ_BDS*L2_FREQ_BDS/
    				   ( L2_FREQ_BDS*L2_FREQ_BDS - L7_FREQ_BDS*L7_FREQ_BDS);
    			   double BDS_b = L7_FREQ_BDS*L7_FREQ_BDS/
    				   ( L2_FREQ_BDS*L2_FREQ_BDS - L7_FREQ_BDS*L7_FREQ_BDS);

                   if(mgexDCBStore!=NULL)
                   {
    			       // Second, find the DCB(Not all)
    			       double BDS_c2ic7i(0.0), BDS_c2ic6i(0.0);
    			       try
    			       {
    			           // dual-frequency
    			           BDS_c2ic7i = (*mgexDCBStore).getSatDCB(time,sat,"C2I-C7I").value;
    			           BDS_c2ic6i = (*mgexDCBStore).getSatDCB(time,sat,"C2I-C6I").value;

    			       }
    			       catch(...)
    			       {
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }


    			       // Then correct dcb for BDS B1
    			       if((*it).second.find(TypeID::C2I)!=(*it).second.end()) // I
    			       {
    			           // C2I->C2I/C7I IF
    			           it->second[TypeID::C2] = it->second[TypeID::C2I];
    			           it->second[TypeID::C2] += BDS_b*BDS_c2ic7i*C_MPS*1.0E-9;
    			       }
    			       else
    			       {
//                         std::cout << "13" << std::endl;
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }

    			       // Then correct dcb for BDS B2
    			       if((*it).second.find(TypeID::C7I)!=(*it).second.end()) // I
    			       {
    			           // C7I->C2I/C7I IF
    			           it->second[TypeID::C7] = it->second[TypeID::C7I];
    			           it->second[TypeID::C7] += BDS_a*BDS_c2ic7i*C_MPS*1.0E-9;
    			       }
    			       else
    			       {
//                         std::cout << "14" << std::endl;
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }

    			       // Then correct dcb for BDS B3
    			       if((*it).second.find(TypeID::C6I)!=(*it).second.end()) // I
    			       {
    			           // C6I->C2I/C7I IF
    			           it->second[TypeID::C6] = it->second[TypeID::C6I];
    			           it->second[TypeID::C6] += BDS_c2ic6i*C_MPS*1.0E-9 + BDS_b*BDS_c2ic7i*C_MPS*1.0E-9;
    			       }
                   }
                   // for ionosphere-free two-frequency positioning, dcb is not needed.
                   else
                   {
    			       // Then correct dcb for BDS B1
    			       if((*it).second.find(TypeID::C2I)!=(*it).second.end()) // I
    			       {
    			           // C2I->C2I/C7I IF
    			           it->second[TypeID::C2] = it->second[TypeID::C2I];
    			       }
    			       else
    			       {
//                         std::cout << "13" << std::endl;
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }

    			       // Then correct dcb for BDS B2
    			       if((*it).second.find(TypeID::C7I)!=(*it).second.end()) // I
    			       {
    			           // C7I->C2I/C7I IF
    			           it->second[TypeID::C7] = it->second[TypeID::C7I];
    			       }
    			       else
    			       {
//                         std::cout << "14" << std::endl;
    			           satRejectedSet.insert(sat);
    			           continue;
    			       }
                   }

			   }
			   else
			   {
//                 std::cout << "15" << std::endl;
				   // Other system: do nothing because of no dcb corrections.
				   // other satellite system: remove directely
				   satRejectedSet.insert( (*it).first );

				   continue;
			   }

		   }  // End of 'for (it = gData.begin(); it != gData.end(); ++it)'

		   // Let's remove satellites without all TypeID's
		   gData.removeSatID(satRejectedSet);

		   return gData;
	   }
	   catch(RecTypeNotFound& u)
	   {
		   // Throw an exception if something unexpected happens
		   ProcessingException e( getClassName() + ":"
			   + u.what() );

		   GPSTK_THROW(e);
	   }
	   catch(Exception& u)
	   {

		   // Throw an exception if something unexpected happens
		   ProcessingException e( getClassName() + ":"
			   + u.what() );

		   GPSTK_THROW(e);

	   }

   }  // End of method 'MGEXDCBCorrection::Process()'

   void MGEXDCBCorrection::Process(commonHeader& comHeader, gnssRinex& gData)
   {
           // set dcb file pointer from ggd;
        mgexDCBStore  = comHeader.pMGEXDCBStore;
        Process(gData.header.epoch, gData.body);
   };

   void MGEXDCBCorrection::Process(gnssRinex& gData)
   {
        Process(gData.header.epoch, gData.body);
   };

    /** Return a sourceRinex object, adding the new data generated
     *  when calling this object.
     *
     * @param gData     Data object holding the data.
     */
    void MGEXDCBCorrection::Process(commonHeader& comHeader,sourceRinex& gData) 
    {
           // set dcb file pointer from ggd;
        mgexDCBStore  = comHeader.pMGEXDCBStore;

        for( sourceDataMap::iterator sdmIt = gData.body.begin();
             sdmIt != gData.body.end();
             ++sdmIt )
        {
            Process( gData.header.epoch, sdmIt->second );
        }

    }  // End of method 'MGEXDCBCorrection::Process()'


}  // End of namespace gpstk

